/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package javatest1;

/**
 *
 * @author paul
 */

public class RoboAutopilot extends RoboTask {
    
    // sub-tasks for autonomous mode
    class Track extends RoboTask{
        double forkTime = 0;
        double maxTime = 0;
        double accelTime = 0.5;
        double speed1 = 1.0;
        double time1 = 0;
        double speed2 = 0.5;
        double time2 = 0;

        boolean trackLeft = true;
        private boolean lineIsLeft = false;
        private boolean lineIsRight = false;

        Track(double timeTillFork, double mt){
            forkTime = timeTillFork;
            maxTime = mt;
        }

        void start(){
            super.start();
            //robot.drive.resetGyro();
        }

        void handle(){
            if(!active) return;
            super.handle();
            // System.out.printf("T=%1.2f: autopilot tracking\n", taskTime);

            // speed control
            if(taskTime < accelTime){
                // accelerate
                robot.drive.ySpeed = speed1 * (taskTime / accelTime);
            }
            else if(taskTime < time1){
                robot.drive.ySpeed = speed1;
            }
            else if(taskTime < (time1 + time2)){
                robot.drive.ySpeed = speed2;
            }
            else {
                robot.drive.ySpeed = 0;
            }

            if(taskTime >= forkTime && robot.lineTracker.cross){
                robot.drive.go(0, 0, 0);
                System.out.printf("T=%1.2f: autopilot tracking CROSS\n", taskTime);
                stop();
                return;
            }

            // diagonal segment detection (based on sideways motion)
            boolean onDiagonal = (Math.abs(robot.drive.xSpeed) >= 0.2);
            System.out.printf("Autopilot on %s\n", onDiagonal?"DIAGONAL":"STRAIGHT");

            // note: driving backwards steering is reversed
            double turn = 0;
            double slide = 0;
            if(robot.drive.ySpeed > 0){
                turn = onDiagonal?0:0.10;
                slide = 0.05;
            }
            else {
                turn = onDiagonal?0:-0.10;
                slide = 0.05;
            }

            if(trackLeft){
                // tracking left
                if(!robot.lineTracker.left && robot.lineTracker.center){
                    // 0 1 = straight
                    lineIsLeft = false;
                    lineIsRight = false;
                }
                else if(robot.lineTracker.left && !(robot.lineTracker.center && robot.lineTracker.right)){
                    // 1 X = too far right
                    lineIsLeft = true;
                }
                else if(!robot.lineTracker.left && !robot.lineTracker.center && robot.lineTracker.right) {
                    // 0 0 and wasn't left = too far left
                    lineIsRight = true;
                }
                else {
                    lineIsLeft = false;
                    lineIsRight = false;
                }
            }
            else {
                // tracking right
                if(!robot.lineTracker.right && robot.lineTracker.center){
                    // 1 0 = straight
                    lineIsLeft = false;
                    lineIsRight = false;
                }
                else if(robot.lineTracker.right && !(robot.lineTracker.center && robot.lineTracker.left)){
                    // 1 X = too far left
                    lineIsRight = true;
                }
                else if(!robot.lineTracker.right && !robot.lineTracker.center && robot.lineTracker.left) {
                    // 0 0 and wasn't left = too far right
                    lineIsLeft = true;
                }
                else {
                    lineIsLeft = false;
                    lineIsRight = false;
                }
            }

            // actual driving
            if(lineIsLeft){
                // turn/slide left
                robot.drive.rotSpeed = -turn;
                robot.drive.xSpeed += -slide;
                robot.drive.xSpeed = Math.min(robot.drive.xSpeed, 1.0);
                robot.drive.xSpeed = Math.max(robot.drive.xSpeed, -1.0);
            }
            else if(lineIsRight){
                // turn/slide right
                robot.drive.rotSpeed = turn;
                robot.drive.xSpeed += slide;
                robot.drive.xSpeed = Math.min(robot.drive.xSpeed, 1.0);
                robot.drive.xSpeed = Math.max(robot.drive.xSpeed, -1.0);
            }
            else{
                robot.drive.rotSpeed = 0;
                robot.drive.xSpeed *= 0.95; // decay
                //robot.drive.xSpeed = 0;
            }

            // let gyro adjust turn speed to keep straight (if line is in sight)
            if(robot.lineTracker.lineVisible){
                double a = robot.drive.readGyro();
                if(a > 2){
                    robot.drive.rotSpeed = 0.5 * Math.signum(a);
                }
                else if(a < -2)
                {
                    robot.drive.rotSpeed = -0.5 * Math.signum(a);
                }
                else {
                    robot.drive.rotSpeed = 0;
                }
            }
            
            if(taskTime >= maxTime) {
                robot.drive.go(0, 0, 0);
                System.out.printf("T=%1.2f: autopilot tracking stopped\n", taskTime);
                stop();
            }
        }
    }

    class Move extends RoboTask{
        double maxTime = 0;
        double x, y, r;

        Move(double fwd, double side, double rot, double mt){
            y = fwd;
            x = side;
            r = rot;
            maxTime = mt;
        }

        void start(){
            super.start();
            robot.drive.go(y, x, r);
        }

        void handle(){
            if(!active) return;
            super.handle();

            if(taskTime >= maxTime) {
                robot.drive.go(0, 0, 0);
                System.out.printf("T=%1.2f: autopilot move stopped\n", taskTime);
                stop();
            }
        }
    }


    class ReleaseTube extends RoboTask{
        void start(){
            super.start();
            robot.manipulator.moveElevatorTo(robot.manipulator.currentHeight - 6);
        }
        void handle(){
            if(!active) return;
            super.handle();
            if(!robot.manipulator.isMoving()) {
                System.out.printf("T=%1.2f: autopilot tube released\n", taskTime);
                stop();
            }
        }
    }

    class Flip extends RoboTask{
        Flip(){
        }
        void start(){
            super.start();
            robot.drive.go(0, 0, 1);
        }
        void handle(){
            if(!active) return;
            super.handle();
            // do the flip...
            if(taskTime > 2.0) {
                System.out.printf("T=%1.2f: autopilot flipped\n", taskTime);
                robot.drive.go(0, 0, 0);
                stop();
            }
        }
    }

    class Turn extends RoboTask{
        double speed;
        double angle;
        Turn(double a){
            angle = robot.drive.readGyro() + a;
            speed = Math.signum(angle);
        }
        void start(){
            super.start();
            robot.drive.go(0, 0, speed);
        }
        void handle(){
            if(!active) return;
            super.handle();
            double da = Math.abs(angle - robot.drive.readGyro());
            if(da < 2) {
                System.out.printf("T=%1.2f: autopilot turn completed\n", taskTime);
                robot.drive.go(0, 0, 0);
                stop();
            }
            else if(da < 15){
                robot.drive.go(0, 0, speed / 2);
            }
        }
    }

    // the autopilot code

    RoboTask subtask = null;
    int stage = 0;
    double fwdTime = 0;

    double timeToY = 1.9;

    void start(){
        super.start();
        stage = 0;
        subtask = null;
    }

    void handle(){
        if(taskTime > 15.0) {
            robot.drive.go(0, 0, 0);
            stop();
        }
        if(!active) return;
        super.handle();
        if(subtask != null) subtask.handle();

        if(subtask == null || !subtask.active){
            // next task
            switch(stage){
                case 0:
                    // track to put on first ubertube
                    subtask = new Track(1, 7);
                    ((Track)subtask).speed1 = 1.0;
                    ((Track)subtask).time1 = timeToY;
                    ((Track)subtask).speed2 = 0.6;
                    ((Track)subtask).time2 = 4.0;

                    robot.drive.resetGyro();
                    robot.manipulator.goToPeg(6);
                    subtask.start();
                    stage++;
                    break;
                case 1:
                    // get the forward drive time
                    fwdTime = subtask.taskTime;
                    // release the tube
                    subtask = new ReleaseTube();
                    subtask.start();
                    stage++;
                    break;
                case 2:
                    // backtrack
                    subtask = new Track(0.5, fwdTime);
                    ((Track)subtask).speed1 = -0.6;
                    ((Track)subtask).time1 = fwdTime - timeToY; // we go slow first - same time as went forward
                    ((Track)subtask).speed2 = -1.0;
                    ((Track)subtask).time2 = timeToY - 0.25;   // full speed for 3 sec, adjust for time lost due to acceleration
                    robot.manipulator.goToPeg(0);
                    subtask.start();
                    stage++;
                    break;
                case 3:
                    // turn 45 deg
                    subtask = new Turn(45);
                    subtask.start();
                    stage++;
                    break;
                case 4:
                    // move forward and turn to grab the 2nd ubertube
                    subtask = new Move(0.5, 0, 0.0, 0.5);
                    subtask.start();
                    stage++;
                    break;
                case 5:
                    // turn 45 deg
                    subtask = new Turn(-55);
                    subtask.start();
                    stage++;
                    break;
                case 6:
                    // drive until line intercepted
                    subtask = new Track(.5, 7);
                    ((Track)subtask).speed1 = 1.0;
                    ((Track)subtask).time1 = 2.0;
                    ((Track)subtask).speed2 = 0.6;
                    ((Track)subtask).time2 = 4;
                    ((Track)subtask).trackLeft = false;
                    robot.manipulator.goToPeg(6);
                    subtask.start();
                    stage++;
                    stage = 8;
                    break;
                case 7:
                    //subtask = new Track(.7, 7);
                    //robot.drive.go(0.5, 0, 0);
                    //subtask.start();
                    stage++;
                    break;
                case 8:
                    subtask = new ReleaseTube();
                    subtask.start();
                    stage++;
                    break;
                case 9:
                    subtask = new Move(-0.3, 0, 0, 0.5);
                    subtask.start();
                    stage++;
                    break;
                case 10:
                    // backtrack
                    subtask = new Track(0.5, fwdTime);
                    ((Track)subtask).speed1 = -0.6;
                    ((Track)subtask).time1 = fwdTime - timeToY; // we go slow first - same time as went forward
                    ((Track)subtask).speed2 = -1.0;
                    ((Track)subtask).time2 = timeToY - 0.25;   // full speed for 3 sec, adjust for time lost due to acceleration
                    robot.manipulator.goToPeg(1);
                    subtask.start();
                    stage++;
                case 11:
                    break;
                default:
                    stop();
            }
        }
    }

    void simulate(){

    }
}
